/*********************************************************************
*                    SEGGER Microcontroller GmbH                     *
*                        The Embedded Experts                        *
**********************************************************************
*                                                                    *
*            (c) 1995 - 2018 SEGGER Microcontroller GmbH             *
*                                                                    *
*       www.segger.com     Support: support@segger.com               *
*                                                                    *
**********************************************************************
*                                                                    *
*       SEGGER RTT * Real Time Transfer for embedded targets         *
*                                                                    *
**********************************************************************
*                                                                    *
* All rights reserved.                                               *
*                                                                    *
* SEGGER strongly recommends to not make any changes                 *
* to or modify the source code of this software in order to stay     *
* compatible with the RTT protocol and J-Link.                       *
*                                                                    *
* Redistribution and use in source and binary forms, with or         *
* without modification, are permitted provided that the following    *
* conditions are met:                                                *
*                                                                    *
* o Redistributions of source code must retain the above copyright   *
*   notice, this list of conditions and the following disclaimer.    *
*                                                                    *
* o Redistributions in binary form must reproduce the above          *
*   copyright notice, this list of conditions and the following      *
*   disclaimer in the documentation and/or other materials provided  *
*   with the distribution.                                           *
*                                                                    *
* o Neither the name of SEGGER Microcontroller GmbH         *
*   nor the names of its contributors may be used to endorse or      *
*   promote products derived from this software without specific     *
*   prior written permission.                                        *
*                                                                    *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND             *
* CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,        *
* INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF           *
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE           *
* DISCLAIMED. IN NO EVENT SHALL SEGGER Microcontroller BE LIABLE FOR *
* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR           *
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT  *
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;    *
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF      *
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT          *
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE  *
* USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH   *
* DAMAGE.                                                            *
*                                                                    *
**********************************************************************
*                                                                    *
*       RTT version: 6.32c                                           *
*                                                                    *
**********************************************************************
---------------------------END-OF-HEADER------------------------------
File    : SEGGER_RTT_printf.c
Purpose : Replacement for printf to write formatted data via RTT
Revision: $Rev: 9599 $
----------------------------------------------------------------------
*/
#include "SEGGER_RTT.h"
#include "SEGGER_RTT_Conf.h"

/*********************************************************************
*
*       Defines, configurable
*
**********************************************************************
*/

#ifndef SEGGER_RTT_PRINTF_BUFFER_SIZE
#define SEGGER_RTT_PRINTF_BUFFER_SIZE (64)
#endif

#include <stdlib.h>
#include <stdarg.h>


#define FORMAT_FLAG_LEFT_JUSTIFY   (1u << 0)
#define FORMAT_FLAG_PAD_ZERO       (1u << 1)
#define FORMAT_FLAG_PRINT_SIGN     (1u << 2)
#define FORMAT_FLAG_ALTERNATE      (1u << 3)

/*********************************************************************
*
*       Types
*
**********************************************************************
*/

typedef struct {
    char   *  pBuffer;
    unsigned  BufferSize;
    unsigned  Cnt;

    int   ReturnValue;

    unsigned RTTBufferIndex;
} SEGGER_RTT_PRINTF_DESC;

/*********************************************************************
*
*       Function prototypes
*
**********************************************************************
*/
int SEGGER_RTT_vprintf( unsigned BufferIndex, const char * sFormat, va_list * pParamList );

/*********************************************************************
*
*       Static code
*
**********************************************************************
*/
/*********************************************************************
*
*       _StoreChar
*/
static void _StoreChar( SEGGER_RTT_PRINTF_DESC * p, char c )
{
    unsigned Cnt;

    Cnt = p->Cnt;
    if ( ( Cnt + 1u ) <= p->BufferSize ) {
        *( p->pBuffer + Cnt ) = c;
        p->Cnt = Cnt + 1u;
        p->ReturnValue++;
    }
    //
    // Write part of string, when the buffer is full
    //
    if ( p->Cnt == p->BufferSize ) {
        if ( SEGGER_RTT_Write( p->RTTBufferIndex, p->pBuffer, p->Cnt ) != p->Cnt ) {
            p->ReturnValue = -1;
        } else {
            p->Cnt = 0u;
        }
    }
}

/*********************************************************************
*
*       _PrintUnsigned
*/
static void _PrintUnsigned( SEGGER_RTT_PRINTF_DESC * pBufferDesc, unsigned v, unsigned Base, unsigned NumDigits, unsigned FieldWidth, unsigned FormatFlags )
{
    static const char _aV2C[16] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' };
    unsigned Div;
    unsigned Digit;
    unsigned Number;
    unsigned Width;
    char c;

    Number = v;
    Digit = 1u;
    //
    // Get actual field width
    //
    Width = 1u;
    while ( Number >= Base ) {
        Number = ( Number / Base );
        Width++;
    }
    if ( NumDigits > Width ) {
        Width = NumDigits;
    }
    //
    // Print leading chars if necessary
    //
    if ( ( FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY ) == 0u ) {
        if ( FieldWidth != 0u ) {
            if ( ( ( FormatFlags & FORMAT_FLAG_PAD_ZERO ) == FORMAT_FLAG_PAD_ZERO ) && ( NumDigits == 0u ) ) {
                c = '0';
            } else {
                c = ' ';
            }
            while ( ( FieldWidth != 0u ) && ( Width < FieldWidth ) ) {
                FieldWidth--;
                _StoreChar( pBufferDesc, c );
                if ( pBufferDesc->ReturnValue < 0 ) {
                    break;
                }
            }
        }
    }
    if ( pBufferDesc->ReturnValue >= 0 ) {
        //
        // Compute Digit.
        // Loop until Digit has the value of the highest digit required.
        // Example: If the output is 345 (Base 10), loop 2 times until Digit is 100.
        //
        while ( 1 ) {
            if ( NumDigits > 1u ) {     // User specified a min number of digits to print? => Make sure we loop at least that often, before checking anything else (> 1 check avoids problems with NumDigits being signed / unsigned)
                NumDigits--;
            } else {
                Div = v / Digit;
                if ( Div < Base ) {      // Is our divider big enough to extract the highest digit from value? => Done
                    break;
                }
            }
            Digit *= Base;
        }
        //
        // Output digits
        //
        do {
            Div = v / Digit;
            v -= Div * Digit;
            _StoreChar( pBufferDesc, _aV2C[Div] );
            if ( pBufferDesc->ReturnValue < 0 ) {
                break;
            }
            Digit /= Base;
        } while ( Digit );
        //
        // Print trailing spaces if necessary
        //
        if ( ( FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY ) == FORMAT_FLAG_LEFT_JUSTIFY ) {
            if ( FieldWidth != 0u ) {
                while ( ( FieldWidth != 0u ) && ( Width < FieldWidth ) ) {
                    FieldWidth--;
                    _StoreChar( pBufferDesc, ' ' );
                    if ( pBufferDesc->ReturnValue < 0 ) {
                        break;
                    }
                }
            }
        }
    }
}

/*********************************************************************
*
*       _PrintInt
*/
static void _PrintInt( SEGGER_RTT_PRINTF_DESC * pBufferDesc, int v, unsigned Base, unsigned NumDigits, unsigned FieldWidth, unsigned FormatFlags )
{
    unsigned Width;
    int Number;

    Number = ( v < 0 ) ? -v : v;

    //
    // Get actual field width
    //
    Width = 1u;
    while ( Number >= ( int )Base ) {
        Number = ( Number / ( int )Base );
        Width++;
    }
    if ( NumDigits > Width ) {
        Width = NumDigits;
    }
    if ( ( FieldWidth > 0u ) && ( ( v < 0 ) || ( ( FormatFlags & FORMAT_FLAG_PRINT_SIGN ) == FORMAT_FLAG_PRINT_SIGN ) ) ) {
        FieldWidth--;
    }

    //
    // Print leading spaces if necessary
    //
    if ( ( ( ( FormatFlags & FORMAT_FLAG_PAD_ZERO ) == 0u ) || ( NumDigits != 0u ) ) && ( ( FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY ) == 0u ) ) {
        if ( FieldWidth != 0u ) {
            while ( ( FieldWidth != 0u ) && ( Width < FieldWidth ) ) {
                FieldWidth--;
                _StoreChar( pBufferDesc, ' ' );
                if ( pBufferDesc->ReturnValue < 0 ) {
                    break;
                }
            }
        }
    }
    //
    // Print sign if necessary
    //
    if ( pBufferDesc->ReturnValue >= 0 ) {
        if ( v < 0 ) {
            v = -v;
            _StoreChar( pBufferDesc, '-' );
        } else if ( ( FormatFlags & FORMAT_FLAG_PRINT_SIGN ) == FORMAT_FLAG_PRINT_SIGN ) {
            _StoreChar( pBufferDesc, '+' );
        } else {

        }
        if ( pBufferDesc->ReturnValue >= 0 ) {
            //
            // Print leading zeros if necessary
            //
            if ( ( ( FormatFlags & FORMAT_FLAG_PAD_ZERO ) == FORMAT_FLAG_PAD_ZERO ) && ( ( FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY ) == 0u ) && ( NumDigits == 0u ) ) {
                if ( FieldWidth != 0u ) {
                    while ( ( FieldWidth != 0u ) && ( Width < FieldWidth ) ) {
                        FieldWidth--;
                        _StoreChar( pBufferDesc, '0' );
                        if ( pBufferDesc->ReturnValue < 0 ) {
                            break;
                        }
                    }
                }
            }
            if ( pBufferDesc->ReturnValue >= 0 ) {
                //
                // Print number without sign
                //
                _PrintUnsigned( pBufferDesc, ( unsigned )v, Base, NumDigits, FieldWidth, FormatFlags );
            }
        }
    }
}

/*********************************************************************
*
*       Public code
*
**********************************************************************
*/
/*********************************************************************
*
*       SEGGER_RTT_vprintf
*
*  Function description
*    Stores a formatted string in SEGGER RTT control block.
*    This data is read by the host.
*
*  Parameters
*    BufferIndex  Index of "Up"-buffer to be used. (e.g. 0 for "Terminal")
*    sFormat      Pointer to format string
*    pParamList   Pointer to the list of arguments for the format string
*
*  Return values
*    >= 0:  Number of bytes which have been stored in the "Up"-buffer.
*     < 0:  Error
*/
int SEGGER_RTT_vprintf( unsigned BufferIndex, const char * sFormat, va_list * pParamList )
{
    char c;
    SEGGER_RTT_PRINTF_DESC BufferDesc;
    int v;
    unsigned NumDigits;
    unsigned FormatFlags;
    unsigned FieldWidth;
    char acBuffer[SEGGER_RTT_PRINTF_BUFFER_SIZE];

    BufferDesc.pBuffer        = acBuffer;
    BufferDesc.BufferSize     = SEGGER_RTT_PRINTF_BUFFER_SIZE;
    BufferDesc.Cnt            = 0u;
    BufferDesc.RTTBufferIndex = BufferIndex;
    BufferDesc.ReturnValue    = 0;

    do {
        c = *sFormat;
        sFormat++;
        if ( c == 0u ) {
            break;
        }
        if ( c == '%' ) {
            //
            // Filter out flags
            //
            FormatFlags = 0u;
            v = 1;
            do {
                c = *sFormat;
                switch ( c ) {
                    case '-':
                        FormatFlags |= FORMAT_FLAG_LEFT_JUSTIFY;
                        sFormat++;
                        break;
                    case '0':
                        FormatFlags |= FORMAT_FLAG_PAD_ZERO;
                        sFormat++;
                        break;
                    case '+':
                        FormatFlags |= FORMAT_FLAG_PRINT_SIGN;
                        sFormat++;
                        break;
                    case '#':
                        FormatFlags |= FORMAT_FLAG_ALTERNATE;
                        sFormat++;
                        break;
                    default:
                        v = 0;
                        break;
                }
            } while ( v );
            //
            // filter out field with
            //
            FieldWidth = 0u;
            do {
                c = *sFormat;
                if ( ( c < '0' ) || ( c > '9' ) ) {
                    break;
                }
                sFormat++;
                FieldWidth = ( FieldWidth * 10u ) + ( ( unsigned )c - '0' );
            } while ( 1 );

            //
            // Filter out precision (number of digits to display)
            //
            NumDigits = 0u;
            c = *sFormat;
            if ( c == '.' ) {
                sFormat++;
                do {
                    c = *sFormat;
                    if ( ( c < '0' ) || ( c > '9' ) ) {
                        break;
                    }
                    sFormat++;
                    NumDigits = NumDigits * 10u + ( ( unsigned )c - '0' );
                } while ( 1 );
            }
            //
            // Filter out length modifier
            //
            c = *sFormat;
            do {
                if ( ( c == 'l' ) || ( c == 'h' ) ) {
                    sFormat++;
                    c = *sFormat;
                } else {
                    break;
                }
            } while ( 1 );
            //
            // Handle specifiers
            //
            switch ( c ) {
                case 'c': {
                    char c0;
                    v = va_arg( *pParamList, int );
                    c0 = ( char )v;
                    _StoreChar( &BufferDesc, c0 );
                    break;
                }
                case 'd':
                    v = va_arg( *pParamList, int );
                    _PrintInt( &BufferDesc, v, 10u, NumDigits, FieldWidth, FormatFlags );
                    break;
                case 'u':
                    v = va_arg( *pParamList, int );
                    _PrintUnsigned( &BufferDesc, ( unsigned )v, 10u, NumDigits, FieldWidth, FormatFlags );
                    break;
                case 'x':
                case 'X':
                    v = va_arg( *pParamList, int );
                    _PrintUnsigned( &BufferDesc, ( unsigned )v, 16u, NumDigits, FieldWidth, FormatFlags );
                    break;
                case 's': {
                    const char * s = va_arg( *pParamList, const char * );
                    do {
                        c = *s;
                        s++;
                        if ( c == '\0' ) {
                            break;
                        }
                        _StoreChar( &BufferDesc, c );
                    } while ( BufferDesc.ReturnValue >= 0 );
                }
                break;
                case 'p':
                    v = va_arg( *pParamList, int );
                    _PrintUnsigned( &BufferDesc, ( unsigned )v, 16u, 8u, 8u, 0u );
                    break;
                case '%':
                    _StoreChar( &BufferDesc, '%' );
                    break;
                default:
                    break;
            }
            sFormat++;
        } else {
            _StoreChar( &BufferDesc, c );
        }
    } while ( BufferDesc.ReturnValue >= 0 );

    if ( BufferDesc.ReturnValue > 0 ) {
        //
        // Write remaining data, if any
        //
        if ( BufferDesc.Cnt != 0u ) {
            SEGGER_RTT_Write( BufferIndex, acBuffer, BufferDesc.Cnt );
        }
        BufferDesc.ReturnValue += ( int )BufferDesc.Cnt;
    }
    return BufferDesc.ReturnValue;
}

/*********************************************************************
*
*       SEGGER_RTT_printf
*
*  Function description
*    Stores a formatted string in SEGGER RTT control block.
*    This data is read by the host.
*
*  Parameters
*    BufferIndex  Index of "Up"-buffer to be used. (e.g. 0 for "Terminal")
*    sFormat      Pointer to format string, followed by the arguments for conversion
*
*  Return values
*    >= 0:  Number of bytes which have been stored in the "Up"-buffer.
*     < 0:  Error
*
*  Notes
*    (1) Conversion specifications have following syntax:
*          %[flags][FieldWidth][.Precision]ConversionSpecifier
*    (2) Supported flags:
*          -: Left justify within the field width
*          +: Always print sign extension for signed conversions
*          0: Pad with 0 instead of spaces. Ignored when using '-'-flag or precision
*        Supported conversion specifiers:
*          c: Print the argument as one char
*          d: Print the argument as a signed integer
*          u: Print the argument as an unsigned integer
*          x: Print the argument as an hexadecimal integer
*          s: Print the string pointed to by the argument
*          p: Print the argument as an 8-digit hexadecimal integer. (Argument shall be a pointer to void.)
*/
int SEGGER_RTT_printf( unsigned BufferIndex, const char * sFormat, ... )
{
    int r;
    va_list ParamList;

    va_start( ParamList, sFormat );
    r = SEGGER_RTT_vprintf( BufferIndex, sFormat, &ParamList );
    va_end( ParamList );
    return r;
}
/*************************** End of file ****************************/
